#include <WProgram.h>
#include <stdlib.h>
#include <ServoTimer2.h>
#undef abs
#include <Motor.h>
#include <Sonar.h>
//#include <Servo.h>

Sonar frontLeftSensor(5);
Sonar frontSensor(4);
Sonar frontRightSensor(3);
Sonar backRightSensor(6);

ServoTimer2 turret;
Motor motors;

int uvCount = 0;

//Defining constants corresponding to each command (also the ascii code number) 
#define READY 82         //R   AAAR
#define DISTANCE 68      //D   AAAD[L,R,F,B]
#define ANGLE 65         //A
#define SPIN 83          //S   AAAS[L,R]
#define MOVE 77          //M   AAAM[F,R,S]
#define VELOCITY 86      //V
#define GET 71           //G   AAAG[L,R][F,R,S,C]
#define JERK 74          //J   AAAJ[R,L][999]
#define VAL 86           //V   AAAV[R,L][F,R,S][0-9999]
#define UV 85            //U   AAAU

const int arrSize = 4;
/*The three check bytes (used to keep the robot from responding to random serial
 *data) currently "AAA"
 */
#define checkByte1 65   // "A"
#define checkByte2 65   // "A"
#define checkByte3 65   // "A"

void setup()
{
  Serial.begin(9600);

  motors.leftInit(10);
  motors.rightInit(9);
  turret.attach(11);
  motors.stop();
  attachInterrupt(0, count, RISING);
}

//The main program loop
void loop()                     
{
  receiveData();
}

void count()
{
  uvCount++;
}

void receiveData()
{
  int dta, command;                              //variable to hold the serial  byt
  while(Serial.available() < 4){
  };
  dta = Serial.read();
  //Serial.println(dta); 
  if ( dta = checkByte1)
  {                        //Checks for first check byte  
    dta = Serial.read();
    //Serial.println(dta); 
    if ( dta = checkByte2)
    {                    //Checks for second check byte
      dta = Serial.read();
      //Serial.println(" "); 
      if ( dta = checkByte3)
      {                //Checks for third check byte
        command = Serial.read();        //Fourth byte is the command
        //Serial.print(command);
        //Serial.print(".");
        interpretCommand(command);  //sends the parsed request to it&apos;s handler
      }
    }
  }
}


int interpretCommand(int command)
{
  char option, option2;
  char* degree;
  int dist;

  //Serial.println(command);
  if(command == READY)
  {
    Serial.print("ready.");
  }
  else if(command == MOVE)
  {
    while(Serial.available() < 1){
    };
    option = Serial.read();
    switch(option)
    {
    case 'F':
      motors.forward();
      //Serial.print("forward.");
      break;
    case 'B':
      motors.reverse();
      //Serial.print("reverse.");
      break;
    case 'S':
      motors.stop();
      //Serial.print("stop.");
      break;
    }

  }
  else if(command == SPIN)
  {
    while(Serial.available() < 1){
    };
    option = Serial.read();
    switch(option)
    {
    case 'R':
      motors.turnRight();
      //Serial.print("spin right.");
      break;
    case 'L':
      motors.turnLeft();
      //Serial.print("spin left.");
      break;
    }

  }
  else if(command == DISTANCE)
  {
    while(Serial.available() < 2){}
    option = Serial.read();
    option2 = Serial.read();
    
    //Serial.print(option);
    //Serial.print(option2);
    if(option == 'R' && option2 == 'F')
    {
      //Serial.print("Right Front");
      dist = frontRightSensor.ping('c');
    }
    else if(option == 'L' && option2 == 'F')
    {
      //Serial.print("Left Front");
      dist = frontLeftSensor.ping('c');
    }
    else if(option == 'F' && option2 == 'F')
    {
      dist = frontSensor.ping('c');
    }
    
    Serial.println(dist);
  }
  else if(command == ANGLE)
  {
    int angleNum;
    char* angle;
    angle = (char*) malloc(sizeof(char) * arrSize);

    while(Serial.available() < 5){
    };

    for(int i = 0; i < arrSize; i++)
    {
      angle[i] = Serial.read();
    }

    angleNum = atoi(angle);

    Serial.println(angleNum);

    turret.write(angleNum);
  }
  else if(command == VAL)
  {
    int valueNum;
    char* value;
    value = (char*) malloc(sizeof(char) * arrSize);
    char side, dir;

    while(Serial.available() < 6){
    };

    side = Serial.read();
    dir = Serial.read();

    for(int i = 0; i < arrSize; i++)
    {
      value[i] = Serial.read();
    }

    valueNum = atoi(value);

    switch(side)
    {
    case 'L':
      switch(dir)
      {
      case 'F':
        motors.setLeftForward(valueNum);
        break;
      case 'R':
        motors.setLeftReverse(valueNum);
        break;
      case 'S':
        motors.setLeftStop(valueNum);
        break;
      }
      break;
    case 'R':
      switch(dir)
      {
      case 'F':
        motors.setRightForward(valueNum);
        break;
      case 'R':
        motors.setRightReverse(valueNum);
        break;
      case 'S':
        motors.setRightStop(valueNum);
        break;
      }
      break;
    }

  }
  else if(command == GET)
  {
    char side, dir;
    int val;

    while(Serial.available() < 2){
    }

    side = Serial.read();
    dir = Serial.read();
    //Serial.println(type);

    switch(side)
    {
    case 'R':
      switch(dir)
      {
      case 'F':
        val = motors.getRightForward();
        break;
      case 'R':
        val = motors.getRightReverse();
        break;
      case 'S':
        val = motors.getRightStop();
        break;
      case 'C':
        val = motors.getSpeed('r');
        break;
      }
      break;
    case 'L':
      switch(dir)
      {
      case 'F':
        val = motors.getLeftForward();
        break;
      case 'R':
        val = motors.getLeftReverse();
        break;
      case 'S':
        val = motors.getLeftStop();
        break;
      case 'C':
        val = motors.getSpeed('l');
        break;
      }
      break;
    }
    
    Serial.println(val);

  }
  else if(command == UV)
  {
    uvCount = 0;
    int startTime = millis();
    int timeRun = 0;
    
    while(timeRun < 1000)
    {
      timeRun = millis() - startTime;
    }
    
    Serial.println(uvCount);
    uvCount = 0;
  }
  else
  {
    Serial.print("error.");
  }
}

int ctoi(char charIn)
{
  switch(charIn)
  {
  case '0':
    return 0;
  case '1':
    return 1;
  case '2':
    return 2;
  case '3':
    return 3;
  case '4':
    return 4;
  case '5':
    return 5;
  case '6':
    return 6;
  case '7':
    return 7;
  case '8':
    return 8;
  case '9':
    return 9;
  }

}


